#ifndef _MOTION_CONTROL_H_
#define _MOTION_CONTROL_H_

#include "boat_datadef.h"
#include <memory>

// PID控制器类
class PIDController
{
public:
    PIDController(float kp = 0.0f, float ki = 0.0f, float kd = 0.0f);
    ~PIDController() = default;
    
    // 设置PID参数
    void SetParameters(float kp, float ki, float kd, float max_integral = 1000.0f, 
                      float min_output = -1000.0f, float max_output = 1000.0f);
    
    // 计算PID输出
    float Calculate(float setpoint, float measured_value, float dt);
    
    // 重置PID状态
    void Reset();
    
    // 获取各分量
    float GetProportional() const { return proportional_; }
    float GetIntegral() const { return integral_; }
    float GetDerivative() const { return derivative_; }
    float GetLastError() const { return previous_error_; }

private:
    float kp_, ki_, kd_;           // PID参数
    float previous_error_;        // 上次误差
    float integral_;              // 积分项
    float max_integral_;          // 积分限幅
    float min_output_, max_output_; // 输出限幅
    
    float proportional_, derivative_; // 各分量值
    bool first_run_;              // 首次运行标志
    
    // 限幅函数
    float clamp(float value, float min_val, float max_val);
};

// 运动控制类
class MotionControl
{
public:
    MotionControl();
    ~MotionControl() = default;
    
    // 设置控制器类型
    void SetController(int usv_type);
    
    // 设置PID参数
    void SetHeadingPID(float kp, float ki, float kd, float max_integral, 
                       float min_output, float max_output);
    void SetVelocityPID(float kp, float ki, float kd, float max_integral, 
                        float min_output, float max_output);
    
    // 主控制函数
    actuator_control USVControl(const headvel_control& command, const TransState& state, float dt = 0.1f);
    
    // 航向控制
    float HeadingControl(float target_heading, float current_heading, float dt);
    
    // 速度控制
    float VelocityControl(float target_velocity, float current_velocity, float dt);
    
    // 重置控制器
    void Reset();
    
    // 获取控制器状态
    bool IsHeadingControlActive() const { return heading_control_active_; }
    bool IsVelocityControlActive() const { return velocity_control_active_; }

    // 获取PID组件值
    float GetHeadingProportional() const { return heading_pid_->GetProportional(); }
    float GetHeadingIntegral() const { return heading_pid_->GetIntegral(); }
    float GetHeadingDerivative() const { return heading_pid_->GetDerivative(); }
    float GetHeadingLastError() const { return heading_pid_->GetLastError(); }
    
    float GetVelocityProportional() const { return velocity_pid_->GetProportional(); }
    float GetVelocityIntegral() const { return velocity_pid_->GetIntegral(); }
    float GetVelocityDerivative() const { return velocity_pid_->GetDerivative(); }
    float GetVelocityLastError() const { return velocity_pid_->GetLastError(); }
    
    // 获取最后的控制输出
    float GetLastHeadingOutput() const { return last_heading_output_; }
    float GetLastVelocityOutput() const { return last_velocity_output_; }

private:
    int usv_type_;
    bool heading_control_active_;
    bool velocity_control_active_;
    
    std::unique_ptr<PIDController> heading_pid_;
    std::unique_ptr<PIDController> velocity_pid_;
    
    // 辅助函数
    float NormalizeAngle(float angle);
    float CalculateHeadingError(float target, float current);
    actuator_control MapControlToActuator(float heading_output, float velocity_output);
    
    // 滤波和平滑处理
    float heading_output_filter_;
    float velocity_output_filter_;
    float filter_alpha_;

    // 添加成员变量来存储最后的输出
    float last_heading_output_;
    float last_velocity_output_;
};

#endif // _MOTION_CONTROL_H_